/*
 * brushlessControlLoop.c
 *
 *  Created: 20251017
 *  Author: Hecate
 */

#include "brushlessControlLoop.h"

#include <math.h>
#include <stdlib.h>

#include "ClarkeAndParkTransformation.h"
#include "pid.h"

#define PI_FLOAT 3.14159265358979323846f
#define TWO_PI_FLOAT 6.28318530717958647692f

#define _fRange(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))

struct brushlessControlObject
{
    float processFrequencyMs;

    float phaseAPWMDuty;
    float phaseBPWMDuty;
    float phaseCPWMDuty;

    float phasePWMDutyMax;

    float mechanicalAngle;

    float electricalAngleTarget;

    float targetVelocity;
    float targetVelocityMax;

    uint8_t polePairs;

    void *controlObject;
    void (*controlProcess)(brushlessControlObject *obj);
};

static inline void normalizationToTwoPi(float* angle);
static inline float electricalAngleFromMechanicalAngle(float mechanicalAngle, brushlessControlObject *obj);

static void velocityOpenLoopControlProcess(brushlessControlObject *obj);

brushlessControlObject *brushlessControlVelocityOpenLoopInit(float processFre, float phaPwmMax, float tarVel, float tarVelMax, uint8_t polePairs)
{
    brushlessControlObject *obj = (brushlessControlObject *)malloc(sizeof(brushlessControlObject));

    obj->processFrequencyMs = processFre;
    obj->phasePWMDutyMax = phaPwmMax;
    obj->targetVelocity = tarVel;
    obj->targetVelocityMax = tarVelMax;
    obj->polePairs = polePairs;

    obj->phaseAPWMDuty = 0.0f;
    obj->phaseBPWMDuty = 0.0f;
    obj->phaseCPWMDuty = 0.0f;
    obj->mechanicalAngle = 0.0f;
    obj->electricalAngleTarget = 0.0f;
    obj->controlObject = NULL;
    obj->controlProcess = velocityOpenLoopControlProcess;

    return obj;
}

void brushlessControlDeinit(brushlessControlObject *obj)
{
    if (obj)
    {
        free(obj);
    }
}

void brushlessControlProcess(brushlessControlObject *obj)
{
    obj->controlProcess(obj);
}

float brushlessControlGetPhasePWMDuty(const brushlessControlObject *obj, brushlessPhase phase)
{
    return phase == phaseA ? obj->phaseAPWMDuty : (phase == phaseB ? obj->phaseBPWMDuty : obj->phaseCPWMDuty);
}

static void velocityOpenLoopControlProcess(brushlessControlObject *obj)
{
    obj-> mechanicalAngle += _fRange(obj->targetVelocity, -obj->targetVelocityMax, obj->targetVelocityMax) * (obj->processFrequencyMs * 0.001f);
    normalizationToTwoPi(&obj->mechanicalAngle);
    obj->electricalAngleTarget = electricalAngleFromMechanicalAngle(obj->mechanicalAngle, obj);

    float IqTarget = _fRange(obj->targetVelocity, -obj->targetVelocityMax, obj->targetVelocityMax);

    float Ia, Ib, Ic;
    inverseClarkeAndParkTransformation(0.0f, IqTarget, obj->electricalAngleTarget, &Ia, &Ib, &Ic);

    Ia = _fRange(Ia, -obj->phasePWMDutyMax, obj->phasePWMDutyMax);
    Ib = _fRange(Ib, -obj->phasePWMDutyMax, obj->phasePWMDutyMax);
    Ic = _fRange(Ic, -obj->phasePWMDutyMax, obj->phasePWMDutyMax);

    obj->phaseAPWMDuty = 0.5f * (1.0f + Ia);
    obj->phaseBPWMDuty = 0.5f * (1.0f + Ib);
    obj->phaseCPWMDuty = 0.5f * (1.0f + Ic);
}

static inline void normalizationToTwoPi(float* angle)
{
    float modTemp = fmodf(*angle, TWO_PI_FLOAT);

    *angle = (modTemp >= 0.0f) ? modTemp : (modTemp + TWO_PI_FLOAT);
}

static inline float electricalAngleFromMechanicalAngle(float mechanicalAngle, brushlessControlObject *obj)
{
    return mechanicalAngle * obj->polePairs;
}


